#!/bin/bash
# Muto ROS2 Controller - 一键重新构建和启动脚本
# 使用方法: ./rebuild_and_launch.sh

set -e  # 遇到错误时退出

echo "=== Muto ROS2 Controller 一键重新构建和启动 ==="
echo ""

# 强制拉取最新代码
echo "正在强制拉取最新代码..."
echo "保存当前工作区状态..."
git stash push -m "Auto stash before force pull $(date)"
echo "拉取远程最新代码..."
git fetch origin
git reset --hard origin/main
echo "代码更新完成"
echo ""

# 获取脚本所在目录（工作空间根目录）
WORKSPACE_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
echo "工作空间目录: $WORKSPACE_DIR/muto_llm_ws/"
cd "$WORKSPACE_DIR/muto_llm_ws/"

# 步骤1: 清理构建目录
echo "步骤1: 清理构建目录..."
if [ -d "build" ]; then
    echo "删除 build/ 目录"
    rm -rf build/
fi

if [ -d "install" ]; then
    echo "删除 install/ 目录"
    rm -rf install/
fi

if [ -d "log" ]; then
    echo "删除 log/ 目录"
    rm -rf log/
fi

echo "构建目录清理完成"
echo ""

# 步骤2: 检查ROS2环境
echo "步骤2: 检查ROS2环境..."
if [ -z "$ROS_DISTRO" ]; then
    echo "警告: ROS_DISTRO 未设置，尝试source ROS2环境"
    if [ -f "/opt/ros/foxy/setup.bash" ]; then
        source /opt/ros/foxy/setup.bash
        echo "已source ROS2 Foxy环境"
    else
        echo "错误: 找不到ROS2环境，请手动source后再运行此脚本"
        exit 1
    fi
else
    echo "ROS2环境已设置: $ROS_DISTRO"
fi
echo ""

# 步骤2.1: 统一设置并修复 RMW 实现变量（CycloneDDS）
echo "步骤2.1: 设置 RMW_IMPLEMENTATION=rmw_cyclonedds_cpp（修复可能的拼写错误）"
# 如果已设置为错误拼写，自动纠正；未设置则设为正确值
if [ -n "$RMW_IMPLEMENTATION" ]; then
    if [ "$RMW_IMPLEMENTATION" = "rmw_cyclonedx_cpp" ]; then
        echo "检测到错误拼写 rmw_cyclonedx_cpp，正在纠正..."
        export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
    else
        echo "当前 RMW_IMPLEMENTATION=$RMW_IMPLEMENTATION"
    fi
else
    export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
    echo "RMW_IMPLEMENTATION 未设置，已设为 rmw_cyclonedds_cpp"
fi
echo ""

# 步骤3: 构建包
echo "步骤3: 构建ROS2包..."
echo "执行: colcon build --packages-select muto_ros2_controller --symlink-install"
colcon build --packages-select muto_ros2_controller --symlink-install

if [ $? -eq 0 ]; then
    echo "构建成功！"
else
    echo "构建失败，请检查错误信息"
    exit 1
fi
echo ""

# 步骤4: Source工作空间
echo "步骤4: Source工作空间环境..."
if [ -f "install/setup.bash" ]; then
    source install/setup.bash
    echo "已source工作空间环境"
else
    echo "错误: install/setup.bash 不存在，构建可能失败"
    exit 1
fi
echo ""

# 步骤5: 关闭旧的进程
echo "步骤5: 检查并关闭旧的进程..."

# 关闭占用8080端口的进程
echo "检查端口8080占用情况..."
PORT_PID=$(lsof -ti:8080 2>/dev/null || true)
if [ ! -z "$PORT_PID" ]; then
    echo "发现端口8080被进程 $PORT_PID 占用，正在关闭..."
    kill -9 $PORT_PID 2>/dev/null || true
    sleep 2
    echo "端口8080进程已关闭"
else
    echo "端口8080未被占用"
fi

# 关闭可能存在的ROS2节点
echo "检查并关闭旧的ROS2节点..."
ROSNODES=$(ros2 node list 2>/dev/null | grep -E "muto_controller|muto_robot" || true)
if [ ! -z "$ROSNODES" ]; then
    echo "发现运行中的Muto节点: $ROSNODES"
    echo "正在关闭旧节点..."
    
    # 尝试优雅关闭所有相关进程
    pkill -f "muto_controller_node" 2>/dev/null || true
    pkill -f "muto_controller" 2>/dev/null || true
    pkill -f "ros2 launch.*muto" 2>/dev/null || true
    pkill -f "ros2 run.*muto_ros2_controller" 2>/dev/null || true
    sleep 5
    
    # 检查是否还有残留节点
    REMAINING_NODES=$(ros2 node list 2>/dev/null | grep -E "muto_controller|muto_robot" || true)
    if [ ! -z "$REMAINING_NODES" ]; then
        echo "仍有残留节点，强制关闭: $REMAINING_NODES"
        # 强制关闭
        pkill -9 -f "muto_controller_node" 2>/dev/null || true
        pkill -9 -f "muto_controller" 2>/dev/null || true
        pkill -9 -f "ros2 launch.*muto" 2>/dev/null || true
        pkill -9 -f "ros2 run.*muto_ros2_controller" 2>/dev/null || true
        sleep 3
        
        # 最终检查
        FINAL_CHECK=$(ros2 node list 2>/dev/null | grep -E "muto_controller|muto_robot" || true)
        if [ ! -z "$FINAL_CHECK" ]; then
            echo "警告: 仍有节点未完全关闭: $FINAL_CHECK"
            echo "建议手动检查并关闭这些进程"
        else
            echo "所有旧节点已成功关闭"
        fi
    else
        echo "旧的ROS2节点已关闭"
    fi
else
    echo "未发现运行中的Muto节点"
fi

# 额外等待确保进程完全关闭
echo "等待进程完全关闭..."
sleep 2
echo "旧进程清理完成"
echo ""

# Dify 后端网络检查与阻塞等待
DIFY_HOST="${DIFY_HOST:-192.168.12.57}"
DIFY_CHECK_INTERVAL="${DIFY_CHECK_INTERVAL:-3}"
echo "步骤5.5: 检查后端 Dify 网络连接状态 / Checking backend Dify connectivity..."
echo "目标地址: ${DIFY_HOST}"
while true; do
    if ping -c 1 -W 1 "${DIFY_HOST}" >/dev/null 2>&1; then
        echo "已连接到 Dify 后端 / Connected to Dify backend"
        break
    else
        echo "无法连接到 Dify 后端(${DIFY_HOST})。请检查虚拟机中的后端 Dify 服务器是否运行并网络可达。"
        echo "Connection to Dify backend failed (${DIFY_HOST}). Please verify the backend Dify server in the VM is running and reachable."
        echo "将持续重试，按 Ctrl+C 取消 / Retrying continuously, press Ctrl+C to cancel"
        sleep "${DIFY_CHECK_INTERVAL}"
    fi
done

# 步骤6: 启动节点
echo "步骤6: 启动Muto ROS2 Controller..."
echo "执行: ros2 launch muto_ros2_controller muto_controller.launch.py"
echo "提示: 使用 Ctrl+C 停止节点"
echo ""

# 启动launch文件
ros2 launch muto_ros2_controller muto_controller.launch.py
